#include"hnurm_dart/Detector.h"
#include<opencv2/opencv.hpp>
#include<iostream>
#include<vector>
#include<algorithm>
using namespace cv;
using namespace std;

Mat Detector::Color_Recognition(const Mat& img){
    if(img.empty()){
        cout<<"Color_Recognition open image failed!"<<'\n';
        exit(1);
    }

    Mat img_hsv;
    cvtColor(img,img_hsv,COLOR_BGR2HSV);

    Mat mask;
    inRange(img_hsv,taregt_color_hsv_min,taregt_color_hsv_max,mask);

    return mask;
}

Mat Detector::PreTreat(const Mat& img){
    if(img.empty()){
        cout<<"PreTreat open image failed!"<<'\n';
        exit(1);
    }

    Mat channel[3];
    split(img,channel);

    Mat red_mask,blue_mask,green_mask;
    subtract(channel[1],channel[0],red_mask);
    subtract(channel[1],channel[2],blue_mask);

    threshold(channel[1],green_mask,30,255,cv::THRESH_BINARY);
    threshold(red_mask,red_mask,30,255,cv::THRESH_BINARY);
    threshold(blue_mask,blue_mask,30,255,cv::THRESH_BINARY);

   

    Mat mask= red_mask & blue_mask & green_mask;

    bin_img=mask.clone();

    Mat core=getStructuringElement(MORPH_RECT,Size(5,5));
   
    morphologyEx(mask,mask,MORPH_CLOSE,core,cv::Point(-1,-1),3);
    morphologyEx(mask,mask,MORPH_OPEN,core,cv::Point(-1,-1),1);
    // dilate(mask,mask,core);

    img_center_pos=cv::Point2d(img.cols/2.0,img.rows/2.0);
    ver[0]={0.0,0.0};
    ver[1]={(double)img.cols,0.0};
    ver[2]={(double)img.cols,(double)img.rows};
    ver[3]={0.0,(double)img.rows};

    // cout<<"PreTreat success\n";

    return mask;
}

void Detector::Contours_Process(const Mat& img){

    contours.clear();
	findContours(img,contours,RETR_EXTERNAL,CHAIN_APPROX_SIMPLE);
    
    // //合并相近相似轮廓
    // double mergeDistanceThreshold=5.0;
    // for (size_t i = 0; i < contours.size(); ++i) {
    //     for (size_t j = i + 1; j < contours.size(); ++j) {
    //         cv::Moments moments_i = cv::moments(contours[i]);
    //         cv::Moments moments_j = cv::moments(contours[j]);
    //         cv::Point2f center_i(moments_i.m10 / moments_i.m00, moments_i.m01 / moments_i.m00);
    //         cv::Point2f center_j(moments_j.m10 / moments_j.m00, moments_j.m01 / moments_j.m00);

    //         double distance = cv::norm(center_i - center_j);

    //         if (distance < mergeDistanceThreshold) {
    //             //choose the one located in left_bottom
    //             swap(contours[j],contours.back());
    //             contours.pop_back();
    //             --j; 
    //         }
    //     }
    // }

    vector<vector<Point> > contours_poly(contours.size());

    lights.clear();

    for(auto i=0;i<contours.size();i++){
        approxPolyDP(contours[i],contours_poly[i],3,true);

        Rect2f rect=boundingRect(contours_poly[i]);
        double Area=contourArea(contours[i]);
        
        // if(Area<50 || Area>5000*5000) continue;
        // if(!rect.x || !rect.y || !rect.width || !rect.height) continue;
        // if((rect.height/rect.width)<0.7 || (rect.height/rect.width)>1.4) continue;

        lights.push_back(Light(rect,Area));
    }

    // cout<<"find contours success\n";
}

void Detector::Light_Filter(){
 
    if(lights.empty()) return ;

    sort(lights.begin(),lights.end(),[&](Light& a,Light& b){
        return a.area>b.area;
    });

    //delete contained light
    for(int i=0;i<lights.size();i++)
    for(int j=i+1;j<lights.size();j++)
    {
        
        if(lights[i].light_rect.contains(lights[j].center) || lights[i].light_rect.contains(lights[j].dot[0]) 
        || lights[i].light_rect.contains(lights[j].dot[1]) || lights[i].light_rect.contains(lights[j].dot[2])
        || lights[i].light_rect.contains(lights[j].dot[3]))
        {
            if(lights[i].area<lights[j].area){
                std::swap(lights[i],lights.back());
                lights.pop_back();
                i--;
                break;
            }
            else{
                std::swap(lights[j],lights.back());
                lights.pop_back();
                j--;
            }
        }
    }

}

void Detector::detect(const Mat& frame){

    is_find=hnurm_interfaces::msg::TargetState::FIND_TARGET;

    if(frame.empty()){
        cout<<"Light_Recognition open image failed!"<<'\n';

        offset={0.0,0.0};
        is_find=hnurm_interfaces::msg::TargetState::LOST_TARGET;

        return ;
    }

    Mat mask=PreTreat(frame);
    
    Contours_Process(mask);

    Light_Filter();

    if(lights.size()>=1){
        Calculate_Center_Offset(lights[0].center);
    }else{
        cout<<"Light_recoginition failed!\n";
        Find_Fail();
    }

    Data_Process();

    // cout<<"light_recoginition success\n";
}

void Detector::Calculate_Center_Offset(const cv::Point2d& target)
{
    offset=target-img_center_pos;

    if(offset.x<-ver[2].x || offset.x>ver[2].x || offset.y<-ver[2].y || offset.y>ver[2].y)
    {
        Find_Fail();

        printf("Calculate Offset failed!\n");
        return ;
    }

}

void Detector::Find_Fail()
{
    is_find=hnurm_interfaces::msg::TargetState::LOST_TARGET;
    offset={0.0,0.0};
 
    return ;
}

void Detector::Draw(Mat& frame){
    for(auto i=0;i<lights.size();i++){
        rectangle(frame,lights[i].light_rect,Scalar(0,0,255),1);

        line(frame,lights[i].dot[0],lights[i].dot[2],Scalar(0,255,0));
		line(frame,lights[i].dot[1],lights[i].dot[3],Scalar(0,255,0));
        circle(frame,lights[i].center,3,Scalar(0,0,255));
    }

    line(frame,ver[0],ver[2],Scalar(0,255,0));
    line(frame,ver[1],ver[3],Scalar(0,255,0));
    circle(frame,img_center_pos,3,cv::Scalar(0,0,255));

    string text_pos=std::string("x:")+std::to_string(offset.x)+std::string(" y:")+std::to_string(offset.y);
    putText(frame,text_pos,Point(0,100),FONT_HERSHEY_PLAIN,3.0,Scalar(0,0,255));

    string text_find=std::string("find: ")+ std::string(is_find ? "true" : "false");
    putText(frame,text_find,Point(0,150),FONT_HERSHEY_PLAIN,3.0,Scalar(0,0,255));

    // cout<<"Draw success\n";
}

void Detector::Data_Process()
{
    yaw=round(offset.x);
    yaw/=100.0f;

    if(fabs(yaw)>10.0f) yaw=yaw/fabs(yaw)*10.0f;
}


/*
*
*图像坐标系上的xy    -> 镖架控制 yaw 和pitch
*/

/*
*异常数据检测机制，防止错误数据导致机器异常移动造成损坏
*
*/

/*
*
*添加一个新的类，搜索器类，并且实现工具类Timer类用于计时
*
*
*/